#ifndef ARMCONTROLLER_H
#define ARMCONTROLLER_H

#include <Eigen/Eigen>
#include "fsm/arm/fsm.h"
#include "fsm/arm/base_state.h"
//#include <slaves/slavearm.h>
#include "predef.h"
#include <vector>
#include "base_controller.h"

class ArmController:public BaseController
{

public:
    ArmController(MainController *main_controller, const SlaveIndex slave_index);
    virtual ~ArmController() override;

    virtual ErrorCode Update()override;
    virtual bool ParseUsrData(const u_char *buffer, const size_t buffer_len) override;
    virtual bool PackUsrData(u_char *buffer,size_t &buffer_len) override;
public:
    inline int GetJointNum()const{return joint_num_;}

    inline const Eigen::Vector3d GetArmCurEndPos()const{return arm_end_pos_cur_;}
    inline const Eigen::Quaterniond GetArmCurEndQuat()const{return arm_end_quat_cur_;}
    inline const Eigen::Vector3d GetArmCurEndLinearVel()const{return arm_end_v_cur_;}
    inline const Eigen::Vector3d GetArmCurEndAngularVel()const{return arm_end_w_cur_;}
    inline const Eigen::VectorXd GetArmCurEndWrench()const{return arm_end_wrench_cur_;}

    inline double GetHandCurJointPos()const{return hand_joint_pos_cur_;}
    inline double GetHandCurJointForce()const{return hand_joint_force_cur_;}

    inline const Eigen::Vector3d GetArmTargetEndPos()const{return arm_end_pos_target_;}
    inline const Eigen::Quaterniond GetArmTargetEndQuat()const{return arm_end_quat_target_;}
    inline const Eigen::Vector3d GetArmTargetEndLinearVel()const{return arm_end_v_target_;}
    inline const Eigen::Vector3d GetArmTargetEndAngularVel()const{return arm_end_w_target_;}

    inline double GetHandTargetJointPos()const{return hand_joint_pos_target_;}


    /*
     * target setting from master
     */
    //descartes space
    bool SetArmTargetEndPos(const Eigen::Vector3d &p)
    {
        //target_end_pos_ = mapping_ori_m2s_*Eigen::Vector3d(px,py,pz)*mapping_coeff_m2s_+slave_rela_origin_pos_;
        arm_end_pos_target_=p;
        updated_attr_=updated_attr_|Master2Slave::kArmEndPos;
        return true;
    }
    bool SetArmTargetEndPos(const double &px,const double &py,const double &pz)
    {
        //target_end_pos_ = mapping_ori_m2s_*Eigen::Vector3d(px,py,pz)*mapping_coeff_m2s_+slave_rela_origin_pos_;
        arm_end_pos_target_<<px,py,pz;
        updated_attr_=updated_attr_|Master2Slave::kArmEndPos;
        return true;
    }
    bool SetArmTargetEndPos(std::vector<double> &pos)
    {
        if(pos.size()!=3)
            return false;
        arm_end_pos_target_ = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(pos.data(),pos.size());
        updated_attr_=updated_attr_|Master2Slave::kArmEndPos;
        return true;
    }

    bool SetArmTargetEndQuat(const Eigen::Quaterniond &q)
    {
        //target_end_pos_ = mapping_ori_m2s_*Eigen::Vector3d(px,py,pz)*mapping_coeff_m2s_+slave_rela_origin_pos_;
        arm_end_quat_target_=q;
        updated_attr_=updated_attr_|Master2Slave::kArmEndQuat;
        return true;
    }
    bool SetArmTargetEndQuat(const double &w,const double &ax,const double &ay,const double &az)
    {
        //target_end_quat_= mapping_ori_m2s_*Eigen::Quaterniond(w,ax,ay,az);
        arm_end_quat_target_.coeffs()<<ax,ay,az,w;
        updated_attr_=updated_attr_|Master2Slave::kArmEndQuat;
        return true;
    }

    bool SetArmTargetEndPose(const double &px,const double &py,const double &pz,const double &w,const double &ax,const double &ay,const double &az)
    {
        SetArmTargetEndPos(px,py,pz);
        SetArmTargetEndQuat(w,ax,ay,az);
        return true;
    }
    bool SetArmTargetEndPose(const Eigen::Matrix4d& pose)
    {
        SetArmTargetEndPos(pose(0,3),pose(1,3),pose(2,3));
        Eigen::Quaterniond q(static_cast<Eigen::Matrix3d>(pose.block(0,0,3,3)));//Eigen::Quaterniond::Identity();
        SetArmTargetEndQuat(q.w(),q.x(),q.y(),q.z());
        //std::cout<<"slave_target"<<std::endl<<pose<<std::endl<<std::endl;
        return true;
    }

    bool SetArmTargetEndVel(const Eigen::Vector3d &v,const Eigen::Vector3d &w)
    {
        //target_end_v_= mapping_ori_m2s_*Eigen::Vector3d(vx,vy,vz);
        arm_end_v_target_=v;
        arm_end_w_target_=w;
        updated_attr_=updated_attr_|Master2Slave::kArmEndLinearVel|Master2Slave::kArmEndAngularVel;
        return true;
    }
    bool SetArmTargetEndVel(const double &vx,const double &vy,const double &vz, const double &wx,const double &wy,const double &wz)
    {
        //target_end_v_= mapping_ori_m2s_*Eigen::Vector3d(vx,vy,vz);
        arm_end_v_target_<<vx,vy,vz;
        arm_end_w_target_<<wx,wy,wz;
        updated_attr_=updated_attr_|Master2Slave::kArmEndLinearVel|Master2Slave::kArmEndAngularVel;
        return true;
    }

    inline void SetHandTargetJointPos(const double joints_pos)
    {
        hand_joint_pos_target_ = joints_pos;
        updated_attr_=updated_attr_|Master2Slave::kHandJointPos;
    }
    //target from master end


public:
    //inline bool SetM2SMapping(const Eigen::Matrix3d &m){ m2s_transf_=m; s2m_transf_=m.inverse();return true;}

    inline void SetM2SArmMappingCoeffLarge(){m2s_arm_pos_mapping_coeff=large_m2s_mapping_coeff;}
    inline void SetM2SArmMappingCoeffSmall(){m2s_arm_pos_mapping_coeff=small_m2s_mapping_coeff;}
    inline double GetM2SArmMappingCoeff()const{return m2s_arm_pos_mapping_coeff;}


public:
    //inline void SetSlaveOriginPos(const Eigen::Vector3d &pos=Eigen::Vector3d::Zero()){ slave_->SetArmOriginPos(pos);}
    //inline void SetSlaveOriginPos(const double &px =0,const double &py=0,const double &pz=0){SetSlaveOriginPos(Eigen::Vector3d(px,py,pz));}

    void SetOriginPos();    //set current slave pos as origin
    void ResetOriginPos();  //set default slave pos as origin

    void SendTarget2Slave(const Eigen::Matrix4d &master_arm_cur_pose, const double master_handjoint_cur_pos);
    Eigen::Vector3d GetArmForce(const Eigen::Matrix3d &mb2mh) const;
    double GetHandJointForce(const double master_handjoint_cur_pos) const;

    Eigen::Matrix4d Master2Slave(const Eigen::Matrix4d &mb_t_mh);
    Eigen::Matrix4d Slave2Master(const Eigen::Matrix4d &sb_t_sh);

public:
    /*
    Eigen::Vector3d master_origin_pos_;  //fixed
    Eigen::Vector3d master_temp_origin_pos_;  //default value
    Eigen::Vector3d master_cur_pos_;     //updating by hardware device
    Eigen::Vector3d master_target_pos_;  //used by position mode, Zero or calculated by master_target_pos = S2M_TRANSFORM*(slave_cur_pos-slave_origin_pos)+master_origin_pos
    double master_trigger_pos_;
    */
    const static double small_m2s_mapping_coeff;
    const static double large_m2s_mapping_coeff;

    //boost::shared_ptr<SlaveArm> slave_;

private:
    boost::shared_ptr<arm::FSM> fsm_;


    // used for coor transform

private:
    double m2s_arm_pos_mapping_coeff;
    double s2m_arm_force_mapping_coeff;
    double m2s_gripper_pos_mapping_coeff;
    double s2m_gripper_force_mapping_coeff;

    Eigen::Matrix4d tempsb_t_mb_,mh_t_vsh_,sb_t_tempsb_;
    Eigen::Matrix4d mb_t_tempsb_,vsh_t_mh_,tempsb_t_sb_;
    //Eigen::Matrix4d mh2sh,sh2mh;//fixed, the virtual slave hand coordinate relate to (or expressed in) the master handle coordinate
    //Eigen::Matrix4d sb2mb,mb2sb;//fixed, the master base coordinate relate to (or expressed in) the virtual slave base coordinate

    Eigen::Vector3d slave_default_base_origin_;

    //Eigen::Matrix3d m2s_transf_,s2m_transf_;


private://from slave via socket
    const int joint_num_;
    // enum ArmAttrType{kEndPos=0x01,kEndQuat=0x02,kEndLinearVel=0x04,kEndAngularVel=0x08,kEndForce=0x10,kJointPos=0x20,kJointVel=0x40,kJointForce=0x80};
    Eigen::Vector3d arm_end_pos_cur_;
    Eigen::Quaterniond arm_end_quat_cur_;
    Eigen::Vector3d arm_end_v_cur_;
    Eigen::Vector3d arm_end_w_cur_;
    Eigen::VectorXd arm_end_wrench_cur_;

    //enum HandAttrType{kGripperPos=0x01,kGripperVel=0x02,kGripperForce=0x04};
    double hand_joint_pos_cur_;
    double hand_joint_force_cur_;

private://from master by USB
    Eigen::Vector3d arm_end_pos_target_;
    Eigen::Quaterniond arm_end_quat_target_;
    Eigen::Vector3d arm_end_v_target_;
    Eigen::Vector3d arm_end_w_target_;

    double hand_joint_pos_target_;
};
typedef boost::shared_ptr<ArmController> ArmControllerPtr;

#endif // ARMCONTROLLER_H
